/**********************************************************************
Finds the 150 best features in an image and tracks them through the 
next two images.  The sequential mode is set in order to speed
processing.  The features are stored in a feature table, which is then
saved to a text file; each feature list is also written to a PPM file.
**********************************************************************/

#include <stdlib.h>
#include <stdio.h>
#include "pnmio.h"
#include "klt.h"

#include <syslog.h>
#include <cv.h>
#include <cxcore.h>
#include <highgui.h>
#include "egdebug.h"
#include "egluesh.h"

void PrintMat(CvMat *A);

CvMat* lsq_homog( CvPoint2D64f* pts, CvPoint2D64f* mpts, int n );
double homog_xfer_err( CvPoint2D64f pt, CvPoint2D64f mpt, CvMat* H );
CvPoint2D64f persp_xform_pt( CvPoint2D64f pt, CvMat* T );
extern double dist_sq_2D( CvPoint2D64f p1, CvPoint2D64f p2 );
int egFillPts(CvPoint2D64f *pts1, CvPoint2D64f *pts2, KLT_FeatureTable ft, int frame, KLT_FeatureList fl); 
void egGetData(IplImage *img1, unsigned char **data, int *ncols, int *nrows);
void egDrawLines(IplImage *stacked, CvPoint2D64f *pts1, CvPoint2D64f *pts2, int npts);
extern IplImage* stack_imgs( IplImage* img1, IplImage* img2 );
void egShowTc( KLT_TrackingContext tc );
int egFillPtsMat(CvMat **pts1, CvMat **pts2, KLT_FeatureTable ft, int frame, KLT_FeatureList fl);
int egFindMatches(CvPoint2D64f *matched1, CvPoint2D64f *matched2, KLT_FeatureTable ft, int frame, KLT_FeatureList fl);
CvMat* egFindTransform(CvPoint2D64f *matched1, CvPoint2D64f *matched2);

#define REPLACE
#define NFEATURES 150

#define MAXSTR 128

typedef enum {
	FINDFEATURES=0,
	GLUEIMAGE,
	SIFT1,
	SIFT2,
	KDTREE,
	RANSAC,
	MAX_TIME_TABLE_INDEX
} timetableindex_t;

long int timetable[MAX_TIME_TABLE_INDEX];

char timetablestr[MAX_TIME_TABLE_INDEX][MAXSTR] =
{
	"FindFeatures",
	"GlueImage",
	"Sift1",
	"Sift2",
	"KdTree",
	"Ransac"
};

void showTimeTable()
{
	int i;
	for (i=0; i<MAX_TIME_TABLE_INDEX; i++)
	{
		fprintf(stderr,"%s = \t%ld\n",timetablestr[i],timetable[i]);
	}
}

void initTimeTable()
{
	int i;
	for (i=0; i<MAX_TIME_TABLE_INDEX; i++)
	{
		timetable[i] = 0;
	}
}

#ifdef WIN32
int RunExample3()
#else
int main(int argc, char **argv)
#endif
{
  IplImage *ipl_img1, *ipl_img2, *stacked;
  IplImage *newimg;
  unsigned char *ipl_img1_data;
  unsigned char *ipl_img2_data;
  char fnameout[100];
  KLT_TrackingContext tc;
  KLT_FeatureList fl;
  KLT_FeatureTable ft;
  int nFeatures = NFEATURES, nFrames = argc-1;
  int ncols, nrows;
  int i;
  CvPoint2D64f pts1[NFEATURES], pts2[NFEATURES];
  int npts = 0;
   
  EG_INI();
  
  if (argc < 3) 
  {
  	fprintf(stderr,"usage: %s <file1> ... <filen>\n", argv[0]);
  	exit(1);
  }
  printf("Processing %d files\n", argc-1);
  
  tc = KLTCreateTrackingContext();
  tc->mindist = 10; /* default 10 */
//  egShowTc(tc);
  fl = KLTCreateFeatureList(nFeatures);
  ft = KLTCreateFeatureTable(nFrames, nFeatures);
  tc->sequentialMode = TRUE;
  tc->writeInternalImages = FALSE;
  tc->affineConsistencyCheck = -1;  /* set this to 2 to turn on affine consistency check */
   
  ipl_img1 = cvLoadImage( argv[1], 1 );
  if ( ! ipl_img1 )
  {
  	EG_ERR("Can't load img %s", argv[1]);
  	exit(1);
  }

  egGetData(ipl_img1, &ipl_img1_data, &ncols, &nrows);
  if( ! ipl_img1_data )
  {
    EG_ERR( "unable to load image from %s", argv[1] );
    exit(1);
  }
  
  KLTSelectGoodFeatures(tc, ipl_img1_data, ncols, nrows, fl);
  KLTStoreFeatureList(fl, ft, 0);
  KLTWriteFeatureListToPPM(fl, ipl_img1_data, ncols, nrows, "feat001.ppm");

  for (i = 2 ; i < argc ; i++)  {
	ipl_img2 = cvLoadImage( argv[i], 1 );
	if ( ! ipl_img2 )
	{
	  EG_ERR("Can't load img %s", argv[i]);
	  exit(1);
    }
    egGetData(ipl_img2, &ipl_img2_data, &ncols, &nrows);
    if( ! ipl_img2_data )
    {
      EG_ERR( "unable to load image from %s", argv[i] );
      exit(1);
    }
    KLTTrackFeatures(tc, ipl_img1_data, ipl_img2_data, ncols, nrows, fl);

	EG_INF("nFeatures=%d", fl->nFeatures);
    npts = egFindMatches(pts1, pts2, ft, i-2, fl);
    EG_INF("npts=%d",npts);
    if ( npts <= 4 )
    {
	    cvReleaseImage(&ipl_img2);
	    free(ipl_img2_data);
	    EG_INF("Can't compute fundamental matches.");
    	continue;
    }
	
    stacked = stack_imgs( ipl_img1, ipl_img2 );
    /* Debugging. */
	egDrawLines(stacked, pts1, pts2, npts);
	cvNamedWindow( "Xformed", 1 );
	cvShowImage( "Xformed", stacked );
	cvSaveImage("stacked.png", stacked);
	cvWaitKey( 0 );

    if (egGlue(&newimg, ipl_img1, pts1, ipl_img2, pts2, npts))
    {
    	sprintf(fnameout, "newimg%03d.png", i);
    	printf("Newimg created: %s\n", fnameout);
    	cvSaveImage(fnameout, newimg); /* Debugging. */
    	cvReleaseImage(&newimg);
    }
    else
    {
    	fprintf(stderr,"Cant glue images.");
    }
#ifdef REPLACE
    KLTReplaceLostFeatures(tc, ipl_img2_data, ncols, nrows, fl);
#endif
    KLTStoreFeatureList(fl, ft, i-1);
    sprintf(fnameout, "feat%03d.ppm", i);
    KLTWriteFeatureListToPPM(fl, ipl_img2_data, ncols, nrows, fnameout);
    cvReleaseImage(&ipl_img2);
    ipl_img2_data = NULL;
  }

  KLTFreeFeatureTable(ft);
  KLTFreeFeatureList(fl);
  KLTFreeTrackingContext(tc);

  return 0;
}


/*
  Calculates a least-squares planar homography from point correspondeces.
  
  @param pts array of points
  @param mpts array of corresponding points; each pts[i], i=1..n, corresponds
    to mpts[i]
  @param n number of points in both pts and mpts; must be at least 4
  
  @return Returns the 3 x 3 least-squares planar homography matrix that
    transforms points in pts to their corresponding points in mpts or NULL if
    fewer than 4 correspondences were provided
*/
CvMat* lsq_homog( CvPoint2D64f* pts, CvPoint2D64f* mpts, int n )
{
  CvMat* H, * A, * B, X;
  double x[9];
  int i;

  if( n < 4 )
    {
      fprintf( stderr, "Warning: too few points in lsq_homog(), %s line %d\n",
	       __FILE__, __LINE__ );
      return NULL;
    }

  /* set up matrices so we can unstack homography into X; AX = B */
  A = cvCreateMat( 2*n, 8, CV_64FC1 );
  B = cvCreateMat( 2*n, 1, CV_64FC1 );
  X = cvMat( 8, 1, CV_64FC1, x );
  H = cvCreateMat(3, 3, CV_64FC1);
  cvZero( A );
  for( i = 0; i < n; i++ )
    {
      cvmSet( A, i, 0, pts[i].x );
      cvmSet( A, i+n, 3, pts[i].x );
      cvmSet( A, i, 1, pts[i].y );
      cvmSet( A, i+n, 4, pts[i].y );
      cvmSet( A, i, 2, 1.0 );
      cvmSet( A, i+n, 5, 1.0 );
      cvmSet( A, i, 6, -pts[i].x * mpts[i].x );
      cvmSet( A, i, 7, -pts[i].y * mpts[i].x );
      cvmSet( A, i+n, 6, -pts[i].x * mpts[i].y );
      cvmSet( A, i+n, 7, -pts[i].y * mpts[i].y );
      cvmSet( B, i, 0, mpts[i].x );
      cvmSet( B, i+n, 0, mpts[i].y );
    }
  cvSolve( A, B, &X, CV_SVD );
  x[8] = 1.0;
  X = cvMat( 3, 3, CV_64FC1, x );
  cvConvert( &X, H );

  cvReleaseMat( &A );
  cvReleaseMat( &B );
  return H;
}



/*
  Calculates the transfer error between a point and its correspondence for
  a given homography, i.e. for a point x, it's correspondence x', and
  homography H, computes d(x', Hx)^2.
  
  @param pt a point
  @param mpt pt's correspondence
  @param H a homography matrix
  
  @return Returns the transfer error between pt and mpt given H
*/
double homog_xfer_err( CvPoint2D64f pt, CvPoint2D64f mpt, CvMat* H )
{
  CvPoint2D64f xpt = persp_xform_pt( pt, H );
  
  return sqrt( dist_sq_2D( xpt, mpt ) );
}



/*
  Performs a perspective transformation on a single point.  That is, for a
  point (x, y) and a 3 x 3 matrix T this function returns the point
  (u, v), where
  
  [x' y' w']^T = T * [x y 1]^T,
  
  and
  
  (u, v) = (x'/w', y'/w').

  Note that affine transforms are a subset of perspective transforms.
  
  @param pt a 2D point
  @param T a perspective transformation matrix
  
  @return Returns the point (u, v) as above.
*/
CvPoint2D64f persp_xform_pt( CvPoint2D64f pt, CvMat* T )
{
  CvMat XY, UV;
  double xy[3] = { pt.x, pt.y, 1.0 }, uv[3] = { 0 };
  CvPoint2D64f rslt;

  cvInitMatHeader( &XY, 3, 1, CV_64FC1, xy, CV_AUTOSTEP );
  cvInitMatHeader( &UV, 3, 1, CV_64FC1, uv, CV_AUTOSTEP );
  cvMatMul( T, &XY, &UV );
  rslt = cvPoint2D64f( uv[0] / uv[2], uv[1] / uv[2] );

  return rslt;
}

/**
   Calculates the squared distance between two points.
   
   @param p1 a point
   @param p2 another point
*/
double dist_sq_2D( CvPoint2D64f p1, CvPoint2D64f p2 )
{
  double x_diff = p1.x - p2.x;
  double y_diff = p1.y - p2.y;

  return x_diff * x_diff + y_diff * y_diff;
}

/**
 * @returns the matches found.
 *
 */
int egFillPts(CvPoint2D64f *pts1, CvPoint2D64f *pts2, KLT_FeatureTable ft, int frame, KLT_FeatureList fl) 
{
	int i;
	int k = 0;
 	for (i = 0 ; i < fl->nFeatures ; i++)  {
        if (fl->feature[i]->val == 0)
        {
	    	printf("Feature #%d:  (%f,%f) with value of %d\n",
    			    i, fl->feature[i]->x, fl->feature[i]->y, fl->feature[i]->val);
        	pts1[k].x = ft->feature[i][frame]->x;
        	pts1[k].y = ft->feature[i][frame]->y;
        	pts2[k].x = fl->feature[i]->x;
        	pts2[k].y = fl->feature[i]->y;
        	k++;
        }
 	}
 	return k;
}

void PrintMat(CvMat *A)
{
    int i, j, tmp_i;
    char tmp[256], tmp2[256];
    EG_DEB( "Entering %s", __FUNCTION__);
    for (i = 0; i < A->rows; i++)
    {
        switch (CV_MAT_DEPTH(A->type))
        {
        case CV_32F:
        case CV_64F:
        	memset(tmp, 0, 256);
        	memset(tmp2, 0, 256);
            for (j = 0; j < A->cols; j++)
            {
                snprintf(tmp2, 256, "%8.3f ", (float)cvGetReal2D(A, i, j));
                tmp_i = strlen(tmp);
                strncat(tmp + tmp_i, tmp2, 256 - tmp_i - strlen(tmp2));
            }
            EG_INF(tmp);
            break;
        case CV_8U:
            case CV_16U:
		        	memset(tmp, 0, 256);
		        	memset(tmp2, 0, 256);
                    for(j = 0; j < A->cols; j++)
                    {
                        snprintf(tmp2, 256, "%6d", (int)cvGetReal2D(A, i, j));
		                tmp_i = strlen(tmp);
		                strncat(tmp + tmp_i, tmp2, 256 - tmp_i - strlen(tmp2));
                    }
                    EG_INF(tmp);
            break;
        default:
                break;
        }
    }
    EG_DEB("Exiting %s", __FUNCTION__);
}

void egGetData(IplImage *ipl_img1, unsigned char **data, int *ncols, int *nrows)
{
  unsigned char *ipl_data = NULL;
  IplImage *tmp1 = NULL;
  CvSize size1;
  int x, y, step, i;
  tmp1 = cvCreateImage( cvGetSize(ipl_img1), IPL_DEPTH_8U, 3 );
  if ( ! tmp1 )
  {
  	EG_ERR("Can't create tmp1 image");
  	exit(1);
  }
  cvCvtColor( ipl_img1, tmp1, CV_RGB2YCrCb);
  cvGetRawData(ipl_img1, &ipl_data, &step, &size1);
  if ( ! ipl_data) 
  {
  	EG_ERR("Can't get ipl_data");
  	exit(1);
  }
  EG_DEB("ipl_data[0]=%d size1.width=%d size1.height=%d step=%d\n",
  ipl_data[0], size1.width, size1.height, step);
  *data = malloc(size1.width*size1.height);
  if ( ! *data )
  {
  	EG_ERR("Can't allocate data");
  	exit(1);
  }
  i = 0;
  for( y = 0; y < size1.height; y++, ipl_data += step )
    for( x = 0; x < size1.width; x++ )
      (*data)[i++] = ipl_data[x*3];
  cvReleaseImage( &tmp1 );
  *ncols = size1.width;
  *nrows = size1.height;
}

int egGlue(IplImage **newimg, IplImage *img1, CvPoint2D64f* pts1, IplImage *img2, CvPoint2D64f* pts2, int npts  )
{
    IplImage* xformed2, *xformed1;
   	int dispx, dispy; /* x and y displacement */
    CvMat *H, *H_inv, *H_disp, *H_trans;
    IplImage* mask;
//    struct timeval t1;
    static int toblend = 0;
    char toblend_name[256];
 
    H = lsq_homog( pts1, pts2, npts );
    if (NULL != H)
    {
		EG_INF("Affine:");
		PrintMat(H);
    }
    else
    {
    	fprintf(stderr,"Can't find lsq_homog in %d pts.\n", npts);
    }

    CvSize xformed_size;
    if( H )
    {
        H_inv = cvCreateMat(3, 3, H->type);
  		EG_DEB("After cvClone.\n");
        if( H_inv )
        {
        	H_disp = cvCreateMat(3, 3, H->type);
        	H_trans = cvCreateMat(3, 3, H->type);
    		EG_DEB("After cvCreateMat.\n");
        	/* computes the image size after transform. */
			EG_DEB("\nH:");
			PrintMat(H);
		    if ( cvInvert(H, H_inv, CV_SVD) == 0)
		    {
		    	EG_ERR("Can't invert transform.\n");
	            cvReleaseMat( &H );
	            cvReleaseMat( &H_inv );
	            cvReleaseMat( &H_disp );
	            cvReleaseMat( &H_trans );
			    EG_DEB( "Exiting 1 %s", __FUNCTION__);
		    	return 0;
		    }
//			egNorm3x3(H_inv, H_inv);
			EG_DEB("\nH_inv:");
			PrintMat(H_inv);
        	egGetSizeDisp(cvGetSize( img1 ), cvGetSize( img2 ), H_inv, 
        				  &xformed_size.width, &xformed_size.height, &dispx, &dispy);
        	/* resets the displacement the fit all the new image*/
			egBuildH_disp(dispx, dispy, H_disp);
			egBuildH_trans(H_inv, H_disp, H_trans); 
			EG_DEB("\nH_trans:");
			PrintMat(H_trans);
            xformed2 = cvCreateImage( xformed_size, IPL_DEPTH_8U, 3 );
            xformed1 = cvCreateImage( xformed_size, IPL_DEPTH_8U, 3 );
            *newimg = cvCreateImage( xformed_size, IPL_DEPTH_8U, 3 );
            /* Ensure that the both images will be at the same coordinates system */
            cvWarpPerspective( img2, xformed2, H_trans,
                               CV_INTER_LINEAR + CV_WARP_FILL_OUTLIERS,
                               cvScalarAll( 0 ) );
           	snprintf(toblend_name,256,"toblend_%03d.tif", toblend++);
           	cvSaveImage(toblend_name, xformed2);
            EG_DEB("After img2\n");
            cvWarpPerspective( img1, xformed1, H_disp,
                               CV_INTER_LINEAR + CV_WARP_FILL_OUTLIERS,
                               cvScalarAll( 0 ) );
            EG_DEB("After img1\n");
           mask = egComputeMask(xformed1, xformed2);
           cvCopy( xformed1, *newimg, NULL );
           cvCopy( xformed2, *newimg, mask ); /* replaces only some pixels */
           cvReleaseImage( &xformed1 );
           cvReleaseImage( &xformed2 );
           cvReleaseMat( &H_inv );
           cvReleaseMat( &H_disp );
           cvReleaseMat( &H_trans );
        }
        cvReleaseMat( &H );
	    EG_DEB( "Exiting 2 %s", __FUNCTION__);
        return 1;
    }
    else
    {
    	EG_DEB("Can't compute RANSAC.\n");
	    EG_DEB( "Exiting 3 %s", __FUNCTION__);
    	return 0;
    }    
}

IplImage* egComputeMask(IplImage *xformed1, IplImage *xformed2)
{
	IplImage *tmp1, *tmp2;
    EG_DEB( "Entering %s", __FUNCTION__);
	IplImage *mask = cvCreateImage( cvGetSize(xformed1), IPL_DEPTH_8U, 1);
	IplImage *tmp1arr = cvCreateImage( cvGetSize(xformed1), IPL_DEPTH_8U, 1);
	IplImage *tmp2arr = cvCreateImage( cvGetSize(xformed2), IPL_DEPTH_8U, 1);
	tmp1 = cvCreateImage( cvGetSize(xformed1), IPL_DEPTH_8U, 3 );
	tmp2 = cvCreateImage( cvGetSize(xformed2), IPL_DEPTH_8U, 3 );
	cvCvtColor( xformed1, tmp1, CV_RGB2YCrCb);
	cvCvtColor( xformed2, tmp2, CV_RGB2YCrCb);
	cvSplit(tmp1, tmp1arr, NULL, NULL, NULL);
	cvSplit(tmp2, tmp2arr, NULL, NULL, NULL);
	cvCmp( tmp1arr, tmp2arr, mask, CV_CMP_LT );
	cvReleaseImage( &tmp1 ); 
	cvReleaseImage( &tmp2 );
	cvReleaseImage( &tmp1arr );
	cvReleaseImage( &tmp2arr );
    EG_DEB( "Exiting %s", __FUNCTION__);
	return mask;
}

void egGetSizeDisp(CvSize size1, CvSize size2, CvMat* H_inv, int *width, int *height, int *dispx, int *dispy)
{
	int i;
	int minx, miny, maxx, maxy;
    EG_DEB( "Entering %s", __FUNCTION__);
	CvMat *new_dim = cvCreateMat(3, 4, H_inv->type);
	egGuessNewDim(size2, H_inv, new_dim);
	EG_DEB("\nnew_dim:");
	PrintMat(new_dim);
	minx = 0;
	miny = 0;
	maxx = size1.width; 
	maxy = size1.height; 
	for (i=1; i < new_dim->cols; i++)
	{
		if (minx > cvmGet(new_dim, 0, i))
		{
			minx = cvmGet(new_dim, 0, i);
		}
		if (maxx < cvmGet(new_dim, 0, i))
		{
			maxx = cvmGet(new_dim, 0, i);
		}
		if (miny > cvmGet(new_dim, 1, i))
		{
			miny = cvmGet(new_dim, 1, i);
		}
		if (maxy < cvmGet(new_dim, 1, i))
		{
			maxy = cvmGet(new_dim, 1, i);
		}
	}
	*height = maxy - miny;
	*width = maxx - minx;
	*dispx = minx;
	*dispy = miny;
	cvReleaseMat( &new_dim );
	EG_DEB("height = %d\n", *height);
	EG_DEB("width = %d\n", *width);
	EG_DEB("dispx = %d\n", *dispx);
	EG_DEB("dispy = %d\n", *dispy);
    EG_DEB( "Exiting %s", __FUNCTION__);
}

void egNorm(CvMat* src, CvMat* dst)
{
	int i;
    EG_DEB( "Entering %s", __FUNCTION__);
	assert(src->rows == 3);
	for (i=0; i < src->cols; i++)
	{
		double factor = cvmGet(src, 2, i);
		cvmSet(dst, 0, i, cvmGet(src, 0, i) / factor);
		cvmSet(dst, 1, i, cvmGet(src, 1, i) / factor);
		cvmSet(dst, 2, i, 1.0);
	}
    EG_DEB( "Exiting %s", __FUNCTION__);
}

void egNorm3x3(CvMat* src, CvMat* dst)
{
	int i,j;
	double factor;
    EG_DEB( "Entering %s", __FUNCTION__);
	EG_DEB("rows = %d\n cols = %d", src->rows, src->cols); 
	assert(src->rows == 3 && src->cols == 3);
	factor = cvmGet(src, 2, 2);
	/* TODO: look for a matrix to scalar multiplication. */
	for (i=0; i < src->cols; i++)
	{
		for (j=0; j < src->rows; j++)
		{
			cvmSet(dst, j, i, cvmGet(src, j, i) / factor);
		}
	}
    EG_DEB( "Exiting %s", __FUNCTION__);
}

void egGuessNewDim(CvSize size, CvMat* H_inv, CvMat* new_dim)
{
    EG_DEB( "Entering %s", __FUNCTION__);
	CvMat *dim = cvCreateMat(3, 4, H_inv->type);
	cvmSet(dim, 0, 0, 0);
	cvmSet(dim, 1, 0, 0);
	cvmSet(dim, 2, 0, 1);
	cvmSet(dim, 0, 1, size.width);
	cvmSet(dim, 1, 1, 0);
	cvmSet(dim, 2, 1, 1);
	cvmSet(dim, 0, 2, 0);
	cvmSet(dim, 1, 2, size.height);
	cvmSet(dim, 2, 2, 1);
	cvmSet(dim, 0, 3, size.width);
	cvmSet(dim, 1, 3, size.height);
	cvmSet(dim, 2, 3, 1);
	cvMatMul(H_inv, dim, new_dim);
	egNorm(new_dim, new_dim);
    EG_DEB( "Exiting %s", __FUNCTION__);
}

void egBuildH_disp(int dispx, int dispy, CvMat* H_disp)
{
    EG_DEB( "Entering %s", __FUNCTION__);
	cvmSet(H_disp, 0, 0, 1.0);
	cvmSet(H_disp, 0, 1, 0);
	cvmSet(H_disp, 0, 2, -dispx);
	cvmSet(H_disp, 1, 0, 0);
	cvmSet(H_disp, 1, 1, 1.0);
	cvmSet(H_disp, 1, 2, -dispy);
	cvmSet(H_disp, 2, 0, 0);
	cvmSet(H_disp, 2, 1, 0);
	cvmSet(H_disp, 2, 2, 1.0);
    EG_DEB( "Exiting %s", __FUNCTION__);
}

void egBuildH_trans(CvMat* H_inv, CvMat* H_disp, CvMat* H_trans)
{
    EG_DEB( "Entering %s", __FUNCTION__);
	cvMatMul(H_inv, H_disp, H_trans);
	egNorm3x3(H_trans, H_trans);
    EG_DEB( "Exiting %s", __FUNCTION__);
}

void egDrawLines(IplImage *stacked, CvPoint2D64f *pts1, CvPoint2D64f *pts2, int npts)
{
	CvPoint pt1, pt2;
	int i;
	int height = stacked->height / 2;
	for (i=0; i<npts; i++)
	{
		pt1 = cvPoint( cvRound( pts1[i].x ), cvRound( pts1[i].y ) );
		pt2 = cvPoint( cvRound( pts2[i].x ), cvRound( pts2[i].y + height) );
		cvLine( stacked, pt1, pt2, CV_RGB(255,255,0), 1, 8, 0 );
	}	
}

/*
  Combines two images by scacking one on top of the other
  
  @param img1 top image
  @param img2 bottom image
  
  @return Returns the image resulting from stacking \a img1 on top if \a img2
*/
extern IplImage* stack_imgs( IplImage* img1, IplImage* img2 )
{
  IplImage* stacked = cvCreateImage( cvSize( MAX(img1->width, img2->width),
					     img1->height + img2->height ),
				     IPL_DEPTH_8U, 3 );

  cvZero( stacked );
  cvSetImageROI( stacked, cvRect( 0, 0, img1->width, img1->height ) );
  cvAdd( img1, stacked, stacked, NULL );
  cvSetImageROI( stacked, cvRect(0, img1->height, img2->width, img2->height) );
  cvAdd( img2, stacked, stacked, NULL );
  cvResetImageROI( stacked );

  return stacked;
}


void egShowTc( KLT_TrackingContext tc )
{
	printf("KLT_TrackingContext\n");  /* minimum distance between selected features */
	printf("mindist                =\t%d\n", tc->mindist);
	printf("window_width           =\t%d\n", tc->window_width);         /* dimensions of feature window */
	printf("window_height          =\t%d\n", tc->window_height);                       
	printf("sequentialMode         =\t%d\n", tc->sequentialMode);  /* whether to save most recent image */
	printf("smoothBeforeSelecting  =\t%d\n", tc->smoothBeforeSelecting); /* whether to smooth image before selecting features */
	printf("writeInternalImages    =\t%d\n", tc->writeInternalImages);   /* whether to write internal images for later viewing */
	printf("min_eigenvalue         =\t%d\n", tc->min_eigenvalue);       /* smallest eigenvalue allowed for selecting */
	printf("min_determinant        =\t%f\n", tc->min_determinant);    /* min determinant for declaring tracking failure */
	printf("min_displacement       =\t%f\n", tc->min_displacement);   /* amount of pixel shift for stopping tracking iterations */
	printf("max_iterations         =\t%d\n", tc->max_iterations);       /* max iterations before declaring tracking failure */
	printf("max_residue            =\t%f\n", tc->max_residue);        /* max residue before declaring tracking failure */
	printf("grad_sigma             =\t%f\n", tc->grad_sigma);         /* sigma of gaussian for computing gradient */
	printf("smooth_sigma_fact      =\t%f\n", tc->smooth_sigma_fact);  /* sigma factor of gaussian for smoothing image */
	printf("pyramid_sigma_fact     =\t%f\n", tc->pyramid_sigma_fact); /* sigma factor of gaussian for computing image pyramid */
	printf("nSkippedPixels         =\t%d\n", tc->nSkippedPixels);       /* used to speed up feature selection */
	printf("borderx                =\t%d\n", tc->borderx);              /* border in which features will not be selected, and */
	printf("bordery                =\t%d\n", tc->bordery);              /*    tracked features will be declared out-of-bounds */
	printf("nPyramidLevels         =\t%d\n", tc->nPyramidLevels);       /* number of pyramid levels */
	printf("subsampling            =\t%d\n", tc->subsampling);          /* amount of subsampling between pyramid levels */
}

int egFindMatches(CvPoint2D64f *matched1, CvPoint2D64f *matched2, KLT_FeatureTable ft, int frame, KLT_FeatureList fl)
{
	int num;
	CvMat *status;
	CvMat *points1, *points2; 
	int i,k=0;
	CvMat *fundamental_matrix;

	k = egFillPtsMat(&points1, &points2, ft, frame, fl);
	if (k <= 4)
	{
		EG_INF("Can't compute enough matches.");
		return k;
	} 
	EG_DEB("Total KLT matches = %d", k);
	/* Fill the points here ... */
	status   = cvCreateMat(1,points1->cols,CV_32F);
	fundamental_matrix = cvCreateMat(3,3,CV_32F);
//	num = cvFindFundamentalMat(points1,points2,fundamental_matrix,CV_FM_RANSAC,1.0,0.99,status);
	num = cvFindFundamentalMat(points1,points2,fundamental_matrix,CV_FM_RANSAC,0.5,0.99,status);
//	num = cvFindFundamentalMat(points1,points2,fundamental_matrix,CV_FM_RANSAC,0.2,0.99,status);
//	num = cvFindFundamentalMat(points1,points2,fundamental_matrix,CV_FM_RANSAC,0.5,0.50,status);
	if( num == 1 )
	{
		k = 0;
		for (i=0; i<status->cols; i++)
		{
			if ( cvmGet(status,0,i) == 1.0 )
			{
				k++;
			}
		}
		EG_DEB("Total cvFindFundamentalMat matches = %d", k);
		EG_DEB("status:");
		PrintMat(status);
		k = 0;
		for (i=0; i<status->cols; i++)
		{
			if ( cvmGet(status,0,i) == 1.0 )
			{
				matched1[k].x = cvmGet(points1, 0, i); 
				matched1[k].y = cvmGet(points1, 1, i);
				matched2[k].x = cvmGet(points2, 0, i);		 
				matched2[k].y = cvmGet(points2, 1, i);
				EG_DEB("matched1[%d]=%f,%f", k, matched1[k].x, matched1[k].y);
				EG_DEB("matched2[%d]=%f,%f", k, matched2[k].x, matched2[k].y);
				k++;
			}
		}
	}
	else
	{
		k = 0;
	    EG_INF("Fundamental matrix was not found\n");
	}
	return k;
}

/**
 * @returns the matches found.
 *
 */
int egFillPtsMat(CvMat **pts1, CvMat **pts2, KLT_FeatureTable ft, int frame, KLT_FeatureList fl) 
{
	int i;
	int k = 0;
 	for (i = 0 ; i < fl->nFeatures ; i++)  {
        if (fl->feature[i]->val == 0)
        {
        	k++;
        }
 	}
 	*pts1 = cvCreateMat(3,k,CV_32FC1);
 	*pts2 = cvCreateMat(3,k,CV_32FC1);
 	k = 0;
 	for (i = 0 ; i < fl->nFeatures ; i++)  {
        if (fl->feature[i]->val == 0)
        {
        	cvmSet(*pts1,0,k,ft->feature[i][frame]->x);
        	cvmSet(*pts1,1,k,ft->feature[i][frame]->y);  	
        	cvmSet(*pts1,2,k,1);  	
        	cvmSet(*pts2,0,k,fl->feature[i]->x);
        	cvmSet(*pts2,1,k,fl->feature[i]->y);
        	cvmSet(*pts2,2,k,1);  	
        	k++;
        }
 	}
 	return k;
}

